#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口�?
#include <std_msgs/String.h>
#include <std_msgs/Empty.h> 
#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <cstdlib>  //string转化为double
#include <iomanip>  //保留有效小数
#include<iostream>  
#include <custom_msg/RANGE.h>
#include "commons/rabbit_consts.h"
#include <stdint.h>

using namespace std;

#define min(a,b)                      (((a) < (b)) ? (a) : (b))
static unsigned char rec_buf[SERAIL_READ_MAX_LEN];
serial::Serial ser; //声明串口对象
custom_msg::RANGE RANGE_data;//全局变量，解析后数据
int main11(int argc, char** argv)
{
  ros::init(argc, argv, "serial_node_range");
  ros::NodeHandle nh;
  ros::Publisher RANGE_pub = nh.advertise<custom_msg::RANGE>(TOPIC_RANGE,1000);
  //设置循环的频�?50HZ 20ms 要求循环频率大于数据接收频率
  ros::Rate loop_rate(50);
  int i = 0, start = -1, end = -1,len = 0;
  string strRece,str[6];
  while (ros::ok())
  {
    RANGE_data.header.stamp = ros::Time::now();
    RANGE_data.range=34.12;
    RANGE_data.voltage=34.12;
    RANGE_data.rate=34.12;
    RANGE_pub.publish(RANGE_data);
    ros::spinOnce();
    loop_rate.sleep();
  }
}

//old device lightware 0--100m 
int main_lightware(int argc, char** argv)
{
  ros::init(argc, argv, "serial_node_range");
  ros::NodeHandle nh;
  ros::Publisher RANGE_pub = nh.advertise<custom_msg::RANGE>(TOPIC_RANGE,1000);
  try
  {
    ser.setPort(SERIAL_RANGE_DEV);
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  }
  catch (serial::IOException& e)
  {
    ROS_ERROR_STREAM("Unable to open Serial Port !");
    return -1;
  }
  if (ser.isOpen())
  {
    ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
    return -1;
  }
  //设置循环的频�?50HZ 20ms 要求循环频率大于数据接收频率
  ros::Rate loop_rate(200);
  int i = 0, start = -1, end = -1,len = 0;
  string strRece,str[6];
  while (ros::ok())
  {  
    len = ser.available(); 
    if (len>0) 
    {  
      strRece += ser.read(len); 
      //cout << "read strRece:" << strRece<< "len:" << len<< "\r\n";
      while ( i < strRece.length() )
      {
        start = strRece.find(SERIAL_RANGE_START);
        if ( start == -1)       //not find head 
        {
            if (strRece.length() > 2){  
               strRece = strRece.substr(strRece.length()-3);
            } 
             break;             
        }
        else
        {
          end = strRece.find(SERIAL_RANGE_END);
          if (end == -1)
          {
             if (start != 0){
               strRece = strRece.substr(start);
             }
             break;
          }
          else{
            i = end;
            cout << "all strRece:" << strRece<< "\r\n";
            strRece.substr(start,end+2-start);
            istringstream is(strRece);
            is>>str[0]>>str[1]>>str[2]>>str[3]>>str[4]>>str[5];
            RANGE_data.header.stamp = ros::Time::now();
            RANGE_data.range=atof(str[0].c_str());
            RANGE_data.voltage=atof(str[2].c_str());
            RANGE_data.rate=atoi(str[4].c_str());
            RANGE_pub.publish(RANGE_data);

            if ( i+5 < strRece.length()){
              strRece = strRece.substr(end+2);
            }
            else
            {   
              strRece = "";
            }
            break;
          }
        }
      }
     }
    ros::spinOnce();
    loop_rate.sleep();
  }
}
static int check_sum(int size, unsigned char *buf)
{
    unsigned int sum = 0;
    if(buf[0] == 0x59 && buf[1] == 0x59)
    {
        int i = 0; 
        for(i = 0; i < size - 1; i++)
        {
            sum += buf[i];
        }
        sum &= 0xff;
        if(buf[9] == sum)
        {
            return 0;
        }
        else
        {
            return -1;
        }
    }
    return -1;
}
//new device benewake 0--350m 
int main(int argc, char** argv) {
  int len,val=0,sig=0,i=0,minLen = 0;
  char val_char[50]={0x00};
  ros::init(argc, argv, "serial_node_range");
  ros::NodeHandle nh;
  ros::Publisher RANGE_pub = nh.advertise<custom_msg::RANGE>(TOPIC_RANGE,1000);
  try
  {
    ser.setPort(SERIAL_BW_RANGE_DEV);
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  }
  catch (serial::IOException& e)
  {
    ROS_ERROR_STREAM("Unable to open Serial Port !");
    return -1;
  }
  if (ser.isOpen())
  {
    ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
    return -1;
  }
  ros::Rate loop_rate(200);
  while (ros::ok()) {
    len = ser.available();
    minLen=min(len, SERAIL_READ_MAX_LEN);
    if (minLen >= 9) {
       try
       {
         memset(rec_buf, 0x00, SERAIL_READ_MAX_LEN);
         len = ser.read(rec_buf , minLen); 
       }
       catch (serial::IOException& e)
       {
         ROS_ERROR_STREAM("read Serial Port fail!");
         continue;
       }
      if(*(rec_buf)==0x59&&*(rec_buf+1)==0x59){
        val = *(uint16_t*)(rec_buf+2);
        sig = (float)*(uint16_t*)(rec_buf+4);
        if(sig>=40 && sig<=1200){
          RANGE_data.header.stamp = ros::Time::now();
          RANGE_data.range=val/100.0;
          RANGE_data.voltage=0.0f;
          RANGE_data.rate=sig;
          RANGE_pub.publish(RANGE_data);
        }
        continue;
      }
  }
  /*while (ros::ok()) {
    len = ser.available();
    if (len >= 9) {
      memset(rec_buf, 0x00, sizeof(rec_buf));
      len = ser.read(rec_buf , len); 
      if(*(rec_buf)==0x59&&*(rec_buf+1)==0x59){
        val = *(uint16_t*)(rec_buf+2);
        sig = (float)*(uint16_t*)(rec_buf+4);
        if(sig>=40 && sig<=1200){
          sprintf(val_char,"%.2f", val/100.0f);
          RANGE_data.header.stamp = ros::Time::now();
          RANGE_data.range=atof(val_char);
          RANGE_data.voltage=0.0f;
          RANGE_data.rate=sig;
          RANGE_pub.publish(RANGE_data);
        }
        continue;
      }
    }*/
    ros::spinOnce();
    loop_rate.sleep();
  }
}

